/* Copyright 2007-2008 dnAnalytics Project.
 *
 * Contributors to this file:
 * Marcus Cuda
 *
 * This file is part of dnAnalytics.  dnAnalytics is licensed under the 
 * Microsoft Public License. See License.txt for a complete copy of the
 * license.
 */
using dnAnalytics.Math;

namespace dnAnalytics.LinearAlgebra.Decomposition
{
    internal class UserSvd : AbstractSvd
    {
        private Matrix mMatrix;

        public UserSvd(Matrix matrix, bool mComputeVectors) : base(mComputeVectors)
        {
            mRows = matrix.Rows;
            mColumns = matrix.Columns;
            mMatrix = matrix.Clone();
        }

        protected override void DoCompute()
        {
            int nm = System.Math.Min(mRows + 1, mColumns);
            mS = mMatrix.CreateVector(nm);

            if (mComputeVectors)
            {
                mU = mMatrix.CreateMatrix(mRows, mRows);
                mV = mMatrix.CreateMatrix(mColumns, mColumns);
            }
            mConverged = Decompose();

            //adjust the size of s if row < mColumns.
            //we are using ported copy of linpack's svd code and it uses
            //a singular vector of length mRows+1 when mRows < mColumns.
            //the last element is not used and needs to be removed.
            //we should port lapack's svd routine to remove this problem.
            if (mRows < mColumns)
            {
                nm--;
                Vector tmp = mMatrix.CreateVector(nm);
                for (int i = 0; i < nm; i++)
                {
                    tmp[i] = mS[i];
                }
                mS = tmp;
            }

            double eps = System.Math.Pow(2.0, -52.0);
            double tol = System.Math.Max(mRows, mColumns)*mS[0]*eps;
            mRank = 0;
            for (int h = 0; h < nm; h++)
            {
                if (mS[h] > tol)
                {
                    mRank++;
                }
            }
            //we no longer need the original matrix.
            mMatrix = null;
        }

        private bool Decompose()
        {
            const int MAXITER = 1000;
            double[] e = new double[mColumns];
            double[] work = new double[mRows];

            int i, j;
            int l, lp1;
            double cs = 0.0;
            double sn = 0.0;
            double t;

            int ncu = mRows;

            //   reduce mMatrix to bidiagonal form, storing the diagonal elements
            //   in s and the super-diagonal elements in e.
            int nct = System.Math.Min(mRows - 1, mColumns);
            int nrt = System.Math.Max(0, System.Math.Min(mColumns - 2, mRows));
            int lu = System.Math.Max(nct, nrt);

            for (l = 0; l < lu; l++)
            {
                lp1 = l + 1;
                if (l < nct)
                {
                    // compute the transformation for the l-th column and
                    // place the l-th diagonal in mS[l].
                    double xnorm = Dnrm2Column(mMatrix, mRows, l, l);
                    mS[l] = xnorm;
                    if (!Precision.EqualsWithinDecimalPlaces(mS[l], 0.0, Constants.DecimalsForComparison))
                    {
                        if (!Precision.EqualsWithinDecimalPlaces(mMatrix.ValueAt(l, l), 0.0, Constants.DecimalsForComparison))
                        {
                            mS[l] = Dsign(mS[l], mMatrix.ValueAt(l, l));
                        }
                        DscalColumn(mMatrix, mRows, l, l, 1.0/mS[l]);
                        mMatrix.ValueAt(l, l, (1.0 + mMatrix.ValueAt(l, l)));
                    }

                    mS[l] = -mS[l];
                }

                for (j = lp1; j < mColumns; j++)
                {
                    if (l < nct)
                    {
                        if (!Precision.EqualsWithinDecimalPlaces(mS[l], 0.0, Constants.DecimalsForComparison))
                        {
                            // apply the transformation.
                            t = -Ddot(mMatrix, mRows, l, j, l)/mMatrix.ValueAt(l, l);
                            for (int ii = l; ii < mRows; ii++)
                            {
                                mMatrix.ValueAt(ii, j, mMatrix.ValueAt(ii, j) + t*mMatrix.ValueAt(ii, l));
                            }
                        }
                    }

                    //place the l-th row of mMatrix into  e for the
                    //subsequent calculation of the row transformation.
                    e[j] = mMatrix.ValueAt(l, j);
                }

                if (mComputeVectors && l < nct)
                {
                    // place the transformation in u for subsequent back multiplication.
                    for (i = l; i < mRows; i++)
                    {
                        mU.ValueAt(i, l, mMatrix.ValueAt(i, l));
                    }
                }

                if (l < nrt)
                {
                    // compute the l-th row transformation and place the l-th super-diagonal in e(l).
                    double enorm = Dnrm2Vector(e, lp1);
                    e[l] = enorm;
                    if (!Precision.EqualsWithinDecimalPlaces(e[l], 0.0, Constants.DecimalsForComparison))
                    {
                        if (!Precision.EqualsWithinDecimalPlaces(e[lp1], 0.0, Constants.DecimalsForComparison))
                        {
                            e[l] = Dsign(e[l], e[lp1]);
                        }
                        DscalVector(e, lp1, 1.0/e[l]);
                        e[lp1] = 1.0 + e[lp1];
                    }
                    e[l] = -e[l];

                    if (lp1 < mRows && !Precision.EqualsWithinDecimalPlaces(e[l], 0.0, Constants.DecimalsForComparison))
                    {
                        // apply the transformation.
                        for (i = lp1; i < mRows; i++)
                        {
                            work[i] = 0.0;
                        }

                        for (j = lp1; j < mColumns; j++)
                        {
                            for (int ii = lp1; ii < mRows; ii++)
                            {
                                work[ii] += e[j]*mMatrix.ValueAt(ii, j);
                            }
                        }

                        for (j = lp1; j < mColumns; j++)
                        {
                            double ww = -e[j]/e[lp1];
                            for (int ii = lp1; ii < mRows; ii++)
                            {
                                mMatrix.ValueAt(ii, j, mMatrix.ValueAt(ii, j) + ww*work[ii]);
                            }
                        }
                    }

                    if (mComputeVectors)
                    {
                        // place the transformation in v for subsequent back multiplication.
                        for (i = lp1; i < mColumns; i++)
                        {
                            mV.ValueAt(i, l, e[i]);
                        }
                    }
                }
            }

            //   set up the final bidiagonal mMatrix or order m.
            int m = System.Math.Min(mColumns, mRows + 1);
            int nctp1 = nct + 1;
            int nrtp1 = nrt + 1;
            if (nct < mColumns)
            {
                mS[nctp1 - 1] = mMatrix.ValueAt((nctp1 - 1), (nctp1 - 1));
            }
            if (mRows < m)
            {
                mS[m - 1] = 0.0;
            }
            if (nrtp1 < m)
            {
                e[nrtp1 - 1] = mMatrix.ValueAt((nrtp1 - 1), (m - 1));
            }
            e[m - 1] = 0.0;

            //   if required, generate u.
            if (mComputeVectors)
            {
                for (j = nctp1 - 1; j < ncu; j++)
                {
                    for (i = 0; i < mRows; i++)
                    {
                        mU.ValueAt(i, j, 0.0);
                    }
                    mU.ValueAt(j, j, 1.0);
                }

                for (l = nct - 1; l >= 0; l--)
                {
                    if (!Precision.EqualsWithinDecimalPlaces(mS[l], 0.0, Constants.DecimalsForComparison))
                    {
                        for (j = l + 1; j < ncu; j++)
                        {
                            t = -Ddot(mU, mRows, l, j, l)/mU.ValueAt(l, l);
                            for (int ii = l; ii < mRows; ii++)
                            {
                                mU.ValueAt(ii, j, mU.ValueAt(ii, j) + t*mU.ValueAt(ii, l));
                            }
                        }
                        DscalColumn(mU, mRows, l, l, -1.0);
                        mU.ValueAt(l, l, 1.0 + mU.ValueAt(l, l));
                        for (i = 0; i < l; i++)
                        {
                            mU.ValueAt(i, l, 0.0);
                        }
                    }
                    else
                    {
                        for (i = 0; i < mRows; i++)
                        {
                            mU.ValueAt(i, l, 0.0);
                        }
                        mU.ValueAt(l, l, 1.0);
                    }
                }
            }

            //   if it is required, generate v.
            if (mComputeVectors)
            {
                for (l = mColumns - 1; l >= 0; l--)
                {
                    lp1 = l + 1;
                    if (l < nrt)
                    {
                        if (!Precision.EqualsWithinDecimalPlaces(e[l], 0.0, Constants.DecimalsForComparison))
                        {
                            for (j = lp1; j < mColumns; j++)
                            {
                                t = -Ddot(mV, mColumns, l, j, lp1)/mV.ValueAt(lp1, l);
                                for (int ii = l; ii < mColumns; ii++)
                                {
                                    mV.ValueAt(ii, j, mV.ValueAt(ii, j) + t*mV.ValueAt(ii, l));
                                }
                            }
                        }
                    }
                    for (i = 0; i < mColumns; i++)
                    {
                        mV.ValueAt(i, l, 0.0);
                    }
                    mV.ValueAt(l, l, 1.0);
                }
            }

            //   transform s and e so that they are  double .
            for (i = 0; i < m; i++)
            {
                double r;
                if (!Precision.EqualsWithinDecimalPlaces(mS[i], 0.0, Constants.DecimalsForComparison))
                {
                    t = mS[i];
                    r = mS[i]/t;
                    mS[i] = t;
                    if (i < m - 1)
                    {
                        e[i] = e[i]/r;
                    }
                    if (mComputeVectors)
                    {
                        DscalColumn(mU, mRows, i, 0, r);
                    }
                }
                //   ...exit
                if (i == m - 1)
                {
                    break;
                }
                if (!Precision.EqualsWithinDecimalPlaces(e[i], 0.0, Constants.DecimalsForComparison))
                {
                    t = e[i];
                    r = t/e[i];
                    e[i] = t;
                    mS[i + 1] = mS[i + 1]*r;
                    if (mComputeVectors)
                    {
                        DscalColumn(mV, mColumns, i + 1, 0, r);
                    }
                }
            }

            //   main iteration loop for the singular values.
            int mn = m;
            int iter = 0;

            while (m > 0)
            {
                // quit if all the singular values have been found.
                // if too many iterations have been performed, set
                //      flag and return.
                if (iter >= MAXITER)
                {
                    return false;
                }

                //      this section of the program inspects for
                //      negligible elements in the s and e arrays.  on
                //      completion the variables kase and l are set as follows.

                //         kase = 1     if mS[m] and e[l-1] are negligible and l < m
                //         kase = 2     if mS[l] is negligible and l < m
                //         kase = 3     if e[l-1] is negligible, l < m, and
                //                      mS[l, ..., mS[m] are not negligible (qr step).
                //         kase = 4     if e[m-1] is negligible (convergence).

                double ztest;
                double test;
                for (l = m - 2; l >= 0; l--)
                {
                    test = System.Math.Abs(mS[l]) + System.Math.Abs(mS[l + 1]);
                    ztest = test + System.Math.Abs(e[l]);
                    if (Precision.EqualsWithinDecimalPlaces(ztest, test, Constants.DecimalsForComparison))
                    {
                        e[l] = 0.0;
                        break;
                    }
                }

                int kase;
                if (l == m - 2)
                {
                    kase = 4;
                }
                else
                {
                    int ls;
                    for (ls = m - 1; ls > l; ls--)
                    {
                        test = 0.0;
                        if (ls != m - 1)
                        {
                            test = test + System.Math.Abs(e[ls]);
                        }
                        if (ls != l + 1)
                        {
                            test = test + System.Math.Abs(e[ls - 1]);
                        }
                        ztest = test + System.Math.Abs(mS[ls]);
                        if (Precision.EqualsWithinDecimalPlaces(ztest, test, Constants.DecimalsForComparison))
                        {
                            mS[ls] = 0.0;
                            break;
                        }
                    }

                    if (ls == l)
                    {
                        kase = 3;
                    }
                    else if (ls == m - 1)
                    {
                        kase = 1;
                    }
                    else
                    {
                        kase = 2;
                        l = ls;
                    }
                }

                l = l + 1;

                //      perform the task indicated by kase.
                int k;
                double f;
                switch (kase)
                {
                        // deflate negligible mS[m].
                    case 1:
                        f = e[m - 2];
                        e[m - 2] = 0.0;
                        double t1;
                        for (int kk = l; kk < m - 1; kk++)
                            //for (k = m - 2; k >= 0; k--)
                        {
                            k = m - 2 - kk + l;
                            t1 = mS[k];
                            Drotg(ref t1, ref f, ref cs, ref sn);
                            mS[k] = t1;
                            if (k != l)
                            {
                                f = -sn*e[k - 1];
                                e[k - 1] = cs*e[k - 1];
                            }
                            if (mComputeVectors)
                            {
                                Drot(mV, mColumns, k, m - 1, cs, sn);
                            }
                        }
                        break;

                        // split at negligible mS[l].
                    case 2:
                        f = e[l - 1];
                        e[l - 1] = 0.0;
                        for (k = l; k < m; k++)
                        {
                            t1 = mS[k];
                            Drotg(ref t1, ref f, ref cs, ref sn);
                            mS[k] = t1;
                            f = -sn*e[k];
                            e[k] = cs*e[k];
                            if (mComputeVectors)
                            {
                                Drot(mU, mRows, k, l - 1, cs, sn);
                            }
                        }
                        break;

                        // perform one qr step.
                    case 3:
                        // calculate the shift.
                        double scale = 0.0;
                        scale = System.Math.Max(scale, System.Math.Abs(mS[m - 1]));
                        scale = System.Math.Max(scale, System.Math.Abs(mS[m - 2]));
                        scale = System.Math.Max(scale, System.Math.Abs(e[m - 2]));
                        scale = System.Math.Max(scale, System.Math.Abs(mS[l]));
                        scale = System.Math.Max(scale, System.Math.Abs(e[l]));
                        double sm = mS[m - 1]/scale;
                        double smm1 = mS[m - 2]/scale;
                        double emm1 = e[m - 2]/scale;
                        double sl = mS[l]/scale;
                        double el = e[l]/scale;
                        double b = ((smm1 + sm)*(smm1 - sm) + emm1*emm1)/2.0;
                        double c = (sm*emm1)*(sm*emm1);
                        double shift = 0.0;
                        if (!Precision.EqualsWithinDecimalPlaces(b, 0.0, Constants.DecimalsForComparison) || !Precision.EqualsWithinDecimalPlaces(c, 0.0, Constants.DecimalsForComparison))
                        {
                            shift = System.Math.Sqrt(b*b + c);
                            if (b < 0.0)
                            {
                                shift = -shift;
                            }
                            shift = c/(b + shift);
                        }
                        f = (sl + sm)*(sl - sm) + shift;
                        double g = sl*el;

                        // chase zeros.
                        for (k = l; k < m - 1; k++)
                        {
                            Drotg(ref f, ref g, ref cs, ref sn);
                            if (k != l)
                            {
                                e[k - 1] = f;
                            }
                            f = cs*mS[k] + sn*e[k];
                            e[k] = cs*e[k] - sn*mS[k];
                            g = sn*mS[k + 1];
                            mS[k + 1] = cs*mS[k + 1];
                            if (mComputeVectors)
                            {
                                Drot(mV, mColumns, k, k + 1, cs, sn);
                            }
                            Drotg(ref f, ref g, ref cs, ref sn);
                            mS[k] = f;
                            f = cs*e[k] + sn*mS[k + 1];
                            mS[k + 1] = -sn*e[k] + cs*mS[k + 1];
                            g = sn*e[k + 1];
                            e[k + 1] = cs*e[k + 1];
                            if (mComputeVectors && k < mRows)
                            {
                                Drot(mU, mRows, k, k + 1, cs, sn);
                            }
                        }
                        e[m - 2] = f;
                        iter = iter + 1;
                        break;

                        // convergence.
                    case 4:
                        // make the singular value  positive
                        if (mS[l] < 0.0)
                        {
                            mS[l] = -mS[l];
                            if (mComputeVectors)
                            {
                                DscalColumn(mV, mColumns, l, 0, -1.0);
                            }
                        }

                        // order the singular value.
                        while (l != mn - 1)
                        {
                            if (mS[l] >= mS[l + 1])
                            {
                                break;
                            }
                            t = mS[l];
                            mS[l] = mS[l + 1];
                            mS[l + 1] = t;
                            if (mComputeVectors && l < mColumns)
                            {
                                Dswap(mV, mColumns, l, l + 1);
                            }
                            if (mComputeVectors && l < mRows)
                            {
                                Dswap(mU, mRows, l, l + 1);
                            }
                            l = l + 1;
                        }
                        iter = 0;
                        m = m - 1;
                        break;
                }
            }
            if (mComputeVectors)
            {
                mV = mV.Transpose();
            }
            return true;
        }

        private static double Dsign(double z1, double z2)
        {
            return System.Math.Abs(z1)*(z2/System.Math.Abs(z2));
        }

        private static void Dswap(Matrix A, int mRows, int col1, int col2)
        {
            // swap mColumns Col1,Col2
            for (int i = 0; i < mRows; i++)
            {
                double z = A.ValueAt(i, col1);
                A.ValueAt(i, col1, A.ValueAt(i, col2));
                A.ValueAt(i, col2, z);
            }
        }

        private static void DscalColumn(Matrix A, int mRows, int col, int start, double z)
        {
            // A part of column Col of matrix A from row Start to end multiply by z
            for (int i = start; i < mRows; i++)
            {
                A.ValueAt(i, col, A.ValueAt(i, col)*z);
            }
        }

        private static void DscalVector(double[] A, int start, double z)
        {
            // A part of vector  A from Start to end multiply by z
            for (int i = start; i < A.Length; i++)
            {
                A[i] = A[i]*z;
            }
        }

        private static void Drotg(ref double da, ref double db, ref double c, ref double s)
        {
            // construct givens plane rotation.
            //jack dongarra, linpack, 3/11/78.
            double r, z;

            double roe = db;
            double Absda = System.Math.Abs(da);
            double Absdb = System.Math.Abs(db);
            if (Absda > Absdb)
            {
                roe = da;
            }
            double scale = Absda + Absdb;
            if (Precision.EqualsWithinDecimalPlaces(scale, 0.0, Constants.DecimalsForComparison))
            {
                c = 1.0;
                s = 0.0;
                r = 0.0;
                z = 0.0;
            }
            else
            {
                double sda = da/scale;
                double sdb = db/scale;
                r = scale*System.Math.Sqrt(sda*sda + sdb*sdb);
                if (roe < 0.0)
                {
                    r = -r;
                }
                c = da/r;
                s = db/r;
                z = 1.0;
                if (Absda > Absdb)
                {
                    z = s;
                }
                if (Absdb >= Absda && !Precision.EqualsWithinDecimalPlaces(c, 0.0, Constants.DecimalsForComparison))
                {
                    z = 1.0/c;
                }
            }
            da = r;
            db = z;
            return;
        }

        private static double Dnrm2Column(Matrix A, int mRows, int col, int start)
        {
            double s = 0;
            for (int i = start; i < mRows; i++)
            {
                s += (A.ValueAt(i, col)*A.ValueAt(i, col));
            }
            return System.Math.Sqrt(s);
        }

        private static double Dnrm2Vector(double[] A, int start)
        {
            double s = 0;
            for (int i = start; i < A.Length; i++)
            {
                s += (A[i]*A[i]);
            }
            return System.Math.Sqrt(s);
        }

        private static double Ddot(Matrix A, int mRows, int col1, int col2, int start)
        {
            double z = 0.0;
            for (int i = start; i < mRows; i++)
            {
                z += A.ValueAt(i, col2)*A.ValueAt(i, col1);
            }
            return z;
        }

        private static void Drot(Matrix A, int mRows, int col1, int col2, double c, double s)
        {
            for (int i = 0; i < mRows; i++)
            {
                double z = c*A.ValueAt(i, col1) + s*A.ValueAt(i, col2);
                double tmp = c*A.ValueAt(i, col2) - s*A.ValueAt(i, col1);
                A.ValueAt(i, col2, tmp);
                A.ValueAt(i, col1, z);
            }
        }
    }
}
